/* USER CODE BEGIN Header */
/**
  ******************************************************************************
  * @file           : main.c
  * @brief          : Main program body
  ******************************************************************************
  * @attention
  *
  * Copyright (c) 2022 STMicroelectronics.
  * All rights reserved.
  *
  * This software is licensed under terms that can be found in the LICENSE file
  * in the root directory of this software component.
  * If no LICENSE file comes with this software, it is provided AS-IS.
  *
  ******************************************************************************
  */
/* USER CODE END Header */
/* Includes ------------------------------------------------------------------*/
#include "main.h"
#include "dma.h"
#include "i2c.h"
#include "spi.h"
#include "tim.h"
#include "usart.h"
#include "gpio.h"

/* Private includes ----------------------------------------------------------*/
/* USER CODE BEGIN Includes */
#include "stdio.h"
#include "stdlib.h"
#include "pstwo.h"
#include "NRF24L01.h"
#include "servo.h"
/* USER CODE END Includes */

/* Private typedef -----------------------------------------------------------*/
/* USER CODE BEGIN PTD */
void attitude_put_1(void)
{
    release();
	  HAL_Delay(500);
  	Servo_data(-45,30,20);
	  HAL_Delay(1000);
		Servo_data(-45,25,5);
	  HAL_Delay(1000);
	  hold();
	  HAL_Delay(500);
	  Servo_data(-45,50,40);
		HAL_Delay(500);
		// Servo_data(-18,80,40);
		// HAL_Delay(500);
		// Servo_data(-19,114,-31);//������ɫ����ߣ�
		// HAL_Delay(1000);
		// Target_claw = -120;
		// HAL_Delay(500);
		// Servo_data(-20,40,0);//��ֹ��ȥ��ʱ��ײ��
		// HAL_Delay(700);
		// Servo_data(7,50,25);

}

void attitude_put_2(void)
{
    Target_claw = -120;
   	HAL_Delay(1000);
  	Servo_data(60,15,10);
  	HAL_Delay(1000);
    Servo_data(60,15,1);//ȡ��ɫ
  	Target_claw=-30;
	  HAL_Delay(1000);
	  Servo_data(60,25,20);
		HAL_Delay(1000);
	  Servo_data(17,85,-30);//������ɫ���м䣩
		HAL_Delay(1000);
		Target_claw = -120;
		HAL_Delay(500);
		Servo_data(17,50,-10);//��ֹ��ȥ��ʱ��ײ��
		HAL_Delay(200);
		Servo_data(7,50,25);
}	

void attitude_put_3(void)
{
    Target_claw = -120;
	  HAL_Delay(1000);
  	Servo_data(130,30,20);
		HAL_Delay(1000);
  	Servo_data(130,25,5);
	  Target_claw = -30;
	  HAL_Delay(1000);
  	Servo_data(130,35,30);
		HAL_Delay(500);
		Servo_data(48,125,40);
		HAL_Delay(1000);
		Servo_data(48,130,-29);//���ں�ɫ���ұߣ�
		HAL_Delay(1000);
		Target_claw = -120;
		HAL_Delay(500);
		Servo_data(45,80,0);//��ֹײ��
		HAL_Delay(500);
	  Servo_data(7,50,25);
}

void attitude_peek_3(void)
{
		Servo_data(7,50,25);
	  Target_claw = -120;
	  HAL_Delay(5000);  
    Target_claw = -30;
	  HAL_Delay(1000);
  	Servo_data(130,35,20);
	  HAL_Delay(1000);
	  Servo_data(130,25,5);
	  HAL_Delay(1000);
	  Target_claw = -120;
	  HAL_Delay(1000);
	  Servo_data(130,35,20);
	  HAL_Delay(1000);

}
/* USER CODE END PTD */

/* Private define ------------------------------------------------------------*/
/* USER CODE BEGIN PD */
/* USER CODE END PD */

/* Private macro -------------------------------------------------------------*/
/* USER CODE BEGIN PM */

/* USER CODE END PM */

/* Private variables ---------------------------------------------------------*/

/* USER CODE BEGIN PV */
int count[4]={0};

int last_err[4]={0};

int speed[4]={0};
int err[4]={0};

int tar_pwm[4]={0};
int tar_speed[4]={0};
uint8_t rx_err=0;
char rxmotor[6]={0};

//任务列表
int tar_list[2]={0};
int work=0;
/* USER CODE END PV */

/* Private function prototypes -----------------------------------------------*/
void SystemClock_Config(void);
static void MPU_Config(void);
/* USER CODE BEGIN PFP */

/* USER CODE END PFP */

/* Private user code ---------------------------------------------------------*/
/* USER CODE BEGIN 0 */
void Motor_TIM_PWM_Stop(void);
void Motor_TIM_PWM_Start(void);
void order_pwm(int all_speed[4]);
void change_speed(int _in_x_speed, int _in_y_speed, int _in_yaw_speed);
void limit_speed(int my_speed[4]);
void change_user(char name);
void Motor_forward(void);
void Motor_back(void);
void Motor_left(void);
void Motor_right(void);
void Motor_turn_left(void);
void Motor_turn_right(void);
int Motor_center_max(int _in_number);
int16_t UnsighTosigh (uint8_t law,uint8_t high);
void Servo_PWM_Start();
int fputc(int ch, FILE *f){
 uint8_t temp[1] = {ch};
 HAL_UART_Transmit(&huart2, temp, 1, 0xffff);
return ch;
}

// int fgetc(FILE * f)
// {
//   uint8_t ch = 0;
//   HAL_UART_Receive(&huart1,&ch, 1, 0xffff);
//   return ch;
// }

//int foward_speed[4] = {10, -10, 10, -10};
////int left_speed[4] = {-10, -10, 10, 10};
//int left_speed[4] = {10, 10, -10, -10};
//int turn_left_speed[4] = {10, 10, 10, 10};

int foward_speed[4] = {-1, 1, 1, -1};//���˳���׼ǰ�����򣬷���Ϊ1���������ٶȿɼӴ�����?
//int left_speed[4] = {-10, -10, 10, 10};
int left_speed[4] = {1, 1, -1, -1};//���˳���׼��ƽ�Ʒ���
int turn_left_speed[4] = {-1, -1, -1, -1};//���˳���׼˳ʱ����ת����

int PS2_LX,PS2_LY,PS2_RX,PS2_RY,PS2_KEY;//������һϵ�����α���������ҡ�˵�ģ����

uint8_t Rxbuf[4];//�������ݻ�����
int stop_mode=0;
int is_use_uart = 1;
/* USER CODE END 0 */

/**
  * @brief  The application entry point.
  * @retval int
  */
int main(void)
{
  /* USER CODE BEGIN 1 */

//	uint8_t tmp_buf[33];
  /* USER CODE END 1 */

  /* MPU Configuration--------------------------------------------------------*/
  MPU_Config();

  /* MCU Configuration--------------------------------------------------------*/

  /* Reset of all peripherals, Initializes the Flash interface and the Systick. */
  HAL_Init();

  /* USER CODE BEGIN Init */

  /* USER CODE END Init */

  /* Configure the system clock */
  SystemClock_Config();

  /* USER CODE BEGIN SysInit */

  /* USER CODE END SysInit */

  /* Initialize all configured peripherals */
  MX_GPIO_Init();
  MX_DMA_Init();
  MX_USART1_UART_Init();
  MX_TIM6_Init();
  MX_TIM15_Init();
  MX_SPI1_Init();
  MX_USART2_UART_Init();
  MX_TIM4_Init();
  MX_SPI2_Init();
  MX_TIM2_Init();
  MX_TIM3_Init();
  MX_TIM7_Init();
  MX_I2C1_Init();
  MX_TIM16_Init();
  /* USER CODE BEGIN 2 */
	
  Motor_TIM_PWM_Start();
  printf("hello world");
  HAL_TIM_Base_Start_IT(&htim7);
  HAL_Delay(10);
HAL_UART_Receive_DMA(&huart2, (uint8_t*)rxmotor,6);
  Servo_PWM_Start();
  HAL_TIM_Base_Start_IT(&htim16);
	
//	change_speed(0,400,0);
//  HAL_Delay(2000);
//	change_speed(400,0,0);
//  HAL_Delay(2000);
//	change_speed(0,0,400);
//  HAL_Delay(2000);
//	change_speed(0,0,0);
	
//	change_speed(0,0,30);
//  HAL_Delay(2000);
//	change_speed(30,30,30);
//  HAL_Delay(2000);
	
//	
	
	
  /* USER CODE END 2 */

  /* Infinite loop */
  /* USER CODE BEGIN WHILE */
  //int speed[4]={0,0,0,50};
	uint8_t header;
  while(__HAL_UART_GET_FLAG(&huart2, UART_FLAG_RXNE) != RESET)
  {
    HAL_UART_Receive(&huart2, (uint8_t*)header, 1, 100);
  }
  printf("hello world\n");

	center();
//  pick_car_1();
//	put_car_2();
  while (1)
  {
    /* USER CODE END WHILE */

    /* USER CODE BEGIN 3 */
		
		//HAL_UART_Transmit(&huart2,'a', 1, 0xffff);
		//printf("%d,%d,%d,%d,%d,%d,%d,%d,%d\n",speed[0],speed[1],speed[2],speed[3],tar_speed[0],tar_speed[1],tar_speed[2],tar_speed[3],rx_err);
		//HAL_UART_Receive(&huart2, (uint8_t*)rxmotor, 4, 100);
    //Servo_data(7,50,25);
		//release();
    //Servo_move();
	if(work)
	{
		for(int i=0;i<2;i++)
		{
			switch (tar_list[i])
			{
			case 11://pick from pan
				peek_1();
				break;
			case 12:
				pick_car_1();
				break;
			case 13:
				pick_car_2();
				break;
			case 14:
				pick_car_3();
				break;
			case 15://pick gournd 1
				pick_ground_1();
				break;
			case 16://pick ground 2
				pick_ground_2();
				break;
			case 17://pick ground 3
				pick_ground_3();
				break;
			case 2:
				put_car_1();
				break;
			case 3:
				put_car_2();
				break;
			case 4:
				put_car_3();
				break;
			case 5:
				put_ground_1();
				break;
			case 6:
				put_ground_2();
				break;
			case 7:
				put_ground_3();
				break;
			case 99:
				center();
			break;
			default:
				break;
			}
		}
		work=0;
	}
		HAL_Delay(50);//��Ҫɾ��û�������ʱ��ң����������??????????????
			
  }
  /* USER CODE END 3 */
}

/**
  * @brief System Clock Configuration
  * @retval None
  */
void SystemClock_Config(void)
{
  RCC_OscInitTypeDef RCC_OscInitStruct = {0};
  RCC_ClkInitTypeDef RCC_ClkInitStruct = {0};

  /** Supply configuration update enable
  */
  HAL_PWREx_ConfigSupply(PWR_LDO_SUPPLY);

  /** Configure the main internal regulator output voltage
  */
  __HAL_PWR_VOLTAGESCALING_CONFIG(PWR_REGULATOR_VOLTAGE_SCALE0);

  while(!__HAL_PWR_GET_FLAG(PWR_FLAG_VOSRDY)) {}

  /** Initializes the RCC Oscillators according to the specified parameters
  * in the RCC_OscInitTypeDef structure.
  */
  RCC_OscInitStruct.OscillatorType = RCC_OSCILLATORTYPE_HSE;
  RCC_OscInitStruct.HSEState = RCC_HSE_ON;
  RCC_OscInitStruct.PLL.PLLState = RCC_PLL_ON;
  RCC_OscInitStruct.PLL.PLLSource = RCC_PLLSOURCE_HSE;
  RCC_OscInitStruct.PLL.PLLM = 5;
  RCC_OscInitStruct.PLL.PLLN = 48;
  RCC_OscInitStruct.PLL.PLLP = 2;
  RCC_OscInitStruct.PLL.PLLQ = 2;
  RCC_OscInitStruct.PLL.PLLR = 2;
  RCC_OscInitStruct.PLL.PLLRGE = RCC_PLL1VCIRANGE_2;
  RCC_OscInitStruct.PLL.PLLVCOSEL = RCC_PLL1VCOWIDE;
  RCC_OscInitStruct.PLL.PLLFRACN = 0;
  if (HAL_RCC_OscConfig(&RCC_OscInitStruct) != HAL_OK)
  {
    Error_Handler();
  }

  /** Initializes the CPU, AHB and APB buses clocks
  */
  RCC_ClkInitStruct.ClockType = RCC_CLOCKTYPE_HCLK|RCC_CLOCKTYPE_SYSCLK
                              |RCC_CLOCKTYPE_PCLK1|RCC_CLOCKTYPE_PCLK2
                              |RCC_CLOCKTYPE_D3PCLK1|RCC_CLOCKTYPE_D1PCLK1;
  RCC_ClkInitStruct.SYSCLKSource = RCC_SYSCLKSOURCE_PLLCLK;
  RCC_ClkInitStruct.SYSCLKDivider = RCC_SYSCLK_DIV1;
  RCC_ClkInitStruct.AHBCLKDivider = RCC_HCLK_DIV1;
  RCC_ClkInitStruct.APB3CLKDivider = RCC_APB3_DIV1;
  RCC_ClkInitStruct.APB1CLKDivider = RCC_APB1_DIV1;
  RCC_ClkInitStruct.APB2CLKDivider = RCC_APB2_DIV2;
  RCC_ClkInitStruct.APB4CLKDivider = RCC_APB4_DIV1;

  if (HAL_RCC_ClockConfig(&RCC_ClkInitStruct, FLASH_LATENCY_1) != HAL_OK)
  {
    Error_Handler();
  }
}

/* USER CODE BEGIN 4 */

// code begin by rocket -----------------------------------------------------------------------------------------
void limit_pwm(int pwm[4])
{
	int limit=700;
	for(int i=0;i<4;i++)
	{
		if(pwm[i]>limit)
		{
			pwm[i]=limit;
		}
		if(pwm[i]<-limit)
		{
			pwm[i]=-limit;
		}
	}
}
void HAL_TIM_PeriodElapsedCallback(TIM_HandleTypeDef *htim)
{
  if(htim->Instance==htim16.Instance)
  {
    Servo_move();
    return;
  }

  if(htim->Instance==htim7.Instance)//10hz pid calc
  {
	for(int i=0;i<4;i++)
	{
		speed[i]=count[i];
		err[i]=tar_speed[i]-speed[i];

		tar_pwm[i]+=err[i]*3+(err[i]-last_err[i])*25;//pid

		if(tar_speed[i]==0 && speed[i]==0)
		{
			tar_pwm[i]=0;
		}
		last_err[i]=err[i];
		count[i]=0;
	}
	limit_pwm(tar_pwm);
	order_pwm(tar_pwm);// debug  to decide whether the motor run or not



  }
}

void HAL_GPIO_EXTI_Callback(uint16_t GPIO_Pin)
{
  if(GPIO_Pin == EN1A_Pin)
  {
	  if(HAL_GPIO_ReadPin(EN1B_GPIO_Port,EN1B_Pin)==0)
    {
      count[3]++;
    }
    else
    {
      count[3]--;
    }
    return ;
  }
	if(GPIO_Pin == EN2A_Pin)
  	{
	  	if(HAL_GPIO_ReadPin(EN2B_GPIO_Port,EN2B_Pin)==0)
		{
		count[2]--;
		}
		else
		{
		count[2]++;
		}
		return ;
  	}
  if(GPIO_Pin == EN3A_Pin)
  {
	  if(HAL_GPIO_ReadPin(EN3B_GPIO_Port,EN3B_Pin)==0)
    {
      count[1]++;
    }
    else
    {
      count[1]--;
    }
    return ;
  }
  if(GPIO_Pin == EN4A_Pin)
  {
	  if(HAL_GPIO_ReadPin(EN4B_GPIO_Port,EN4B_Pin)==0)
    {
      count[0]--;
    }
    else
    {
      count[0]++;
    }
    return ;
  }
  
}



//end of code by rocket----------------------------------------------------------------------------------




// Motor_TIM_PWM_Stop(void)���ڹر��ĸ�PWMͨ��
void Motor_TIM_PWM_Stop(void)
{
	HAL_TIM_PWM_Stop(&htim2,TIM_CHANNEL_1);
	HAL_TIM_PWM_Stop(&htim2,TIM_CHANNEL_2);
	HAL_TIM_PWM_Stop(&htim3,TIM_CHANNEL_2);
	HAL_TIM_PWM_Stop(&htim3,TIM_CHANNEL_3);
	
}


void Servo_PWM_Start()
{
  HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_1);
  HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_2);
  HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_3);
  HAL_TIM_PWM_Start(&htim4,TIM_CHANNEL_4);
}


// Motor_TIM_PWM_Start(void)���ڿ���PWMͨ��
void Motor_TIM_PWM_Start(void)
{
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_1); //������ʱ��2ͨ��1PWMͨ��
	HAL_TIM_PWM_Start(&htim2,TIM_CHANNEL_2); //������ʱ��2ͨ��2PWMͨ��
	HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_2); //������ʱ��3ͨ��2PWMͨ��
	HAL_TIM_PWM_Start(&htim3,TIM_CHANNEL_3); //������ʱ��3ͨ��3PWMͨ��
	
	HAL_TIM_PWM_Start(&htim15,TIM_CHANNEL_2);

}

void limit_speed(int my_speed[4])
{
	int return_speed = abs(my_speed[0]);//�õ���һ�����ӵ�PWMֵ
	int max_speed = 0;
	for(int i=1;i<4;i++)
	{
		if(return_speed<abs(my_speed[i]))
		{
			return_speed = abs(my_speed[i]);//�ҵ��ĸ�PWM�е�����?
		}
	}
	if(return_speed>=900)
	{
		max_speed = return_speed;//�������PWMֵ����900
		for(int j=0;j<4;j++)
		{
			my_speed[j]=my_speed[j]*900/max_speed;//���for��䱣֤�˳���??????????????900��PWMֵΪ900��ͬʱ���������ӵ�PWMֵ�������ֲ���
		}
	}
}

/*����Ϊx,y,yaw�����˶�������ٶ�Ȩ��ֵ�������ɸ�������������������һ���������ɿ��������ķ�ֳ������ƶ�*/
void change_speed(int _in_x_speed, int _in_y_speed, int _in_yaw_speed)
{
	// _in_x_speed -999~999
	int target_speed[4] = {0, 0, 0, 0};
	for(int i=0;i<4;i++)
	{
		target_speed[i] = foward_speed[i]*_in_x_speed + left_speed[i]*_in_y_speed + turn_left_speed[i]*_in_yaw_speed;//��ǰ�������ơ�˳ʱ��ת�����Ӧ���ٶ�Ȩ������ڽ���������ӣ����ɻ�������ٶ�
	}
	limit_speed(target_speed);//�õ������ٶȺ�������?
	order_pwm(target_speed);//����ֵ������ٶ�����order_pwm()ʹС���˶�
}


// order_pwm(int all_speed[4])�������ڸı�PWM�Ĵ�С�ͷ���
//����Ĳ��������?4������all_speed[4]��Ϊ�������ĸ����ӵ�PWMֵ�Ĵ�С�����������ű�ʾ������ֵ��ʾ��С
void order_pwm(int all_speed[4])
{
	for(int i=0;i<4;i++)
	{
	  	int need_speed = all_speed[i];//���Ȼ�ȡ���������ÿ�������?
		if(i==1|| i==2)
		{
			need_speed*=-1;
		}

		int pwm_order[3] = {0,0,0}; //����һ������Ϊ3������,��һ������Ԫ�ش洢PWM��Сֵ����2,3������Ԫ�����ڸı�GPIO�ߵ͵�ƽ����
		pwm_order[0] = abs(need_speed);
		
		// ���������if�����ͨ���������ÿ������Ԫ�ص�����������ӦGPIO�ĵ�ƽ����;
		if(need_speed==0)
		{
			pwm_order[1] = 0;
			pwm_order[2] = 0;
		}
		else if(need_speed>0)
		{
			pwm_order[1] = 1;
			pwm_order[2] = 0;
		}
		else if(need_speed<0)
		{
			pwm_order[1] = 0;
			pwm_order[2] = 1;
		}
		//������ĸ�if����Ӧ�ĸ����ӵ�PWMֵ�Լ����ӷ���
		if(i == 1)
		{
			__HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_2,pwm_order[0]);//PA7_LR,�ı�LR�ֵ�ת��
			HAL_GPIO_WritePin(LR_O1_GPIO_Port,LR_O1_Pin,pwm_order[1]);//ͨ��HAL_GPIO_WritePin()�ı�ߵ͵�ƽ�ķ���ı�LR�ֵķ���
	    HAL_GPIO_WritePin(LR_O2_GPIO_Port,LR_O2_Pin,pwm_order[2]);	
		}
		else if(i==0)
		{
		  __HAL_TIM_SET_COMPARE(&htim3,TIM_CHANNEL_3,pwm_order[0]);//PB0_RR,�ı�RR�ֵ�ת��
			HAL_GPIO_WritePin(RR_O1_GPIO_Port,RR_O1_Pin,pwm_order[1]);//ͨ��HAL_GPIO_WritePin()�ı�ߵ͵�ƽ�ķ���ı�RR�ֵķ���
			HAL_GPIO_WritePin(RR_O2_GPIO_Port,RR_O2_Pin,pwm_order[2]);
		}
		else if(i==3)
		{
		  __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_1,pwm_order[0]);//PA0_LF,�ı�LF�ֵ�ת��
			HAL_GPIO_WritePin(LF_O1_GPIO_Port,LF_O1_Pin,pwm_order[1]);//ͨ��HAL_GPIO_WritePin()�ı�ߵ͵�ƽ�ķ���ı�LF�ֵķ���
	    HAL_GPIO_WritePin(LF_O2_GPIO_Port,LF_O2_Pin,pwm_order[2]);
		}
		else if(i==2)
		{
		  __HAL_TIM_SET_COMPARE(&htim2,TIM_CHANNEL_2,pwm_order[0]);//PA1_RF,�ı�RF�ֵ�ת��
			HAL_GPIO_WritePin(RF_O1_GPIO_Port,RF_O1_Pin,pwm_order[1]);//ͨ��HAL_GPIO_WritePin()�ı�ߵ͵�ƽ�ķ���ı�RF�ֵķ���
	    HAL_GPIO_WritePin(RF_O2_GPIO_Port,RF_O2_Pin,pwm_order[2]);
		}
	}
}


//turn right 
//LR is forward_right wheel;LF is back_right wheel;
//RR is forward_Left wheel;RF is back_left wheel;



void delay_us(uint16_t nus)
{
	__HAL_TIM_SET_COUNTER(DLY_TIM_Handle, 0);
	__HAL_TIM_ENABLE(DLY_TIM_Handle);
	while (__HAL_TIM_GET_COUNTER(DLY_TIM_Handle) < nus)
	{
	}
	__HAL_TIM_DISABLE(DLY_TIM_Handle);
}


//����ң����ҡ�˵�ģ����������ң����ģ����ȡֵ��ΧΪ0-255�����������ҡ��ֵת��Ϊ�ٶ��?
int Motor_center_max(int _in_number)
{
	return  300*(_in_number-127) / 127;
}

//���ڲ�ͬң������ҡ����ֵ���ܴ��ڲ��죬��˱�д��κ������ڷ���ʹ�ò�ͬң����
void change_user(char name)
{
	 if(name=='m')
  {
  	change_speed(Motor_center_max(255-PS2_RY), Motor_center_max(255-PS2_RX),Motor_center_max(255-PS2_LX));
  }
  else if(name=='z')
  {
  	change_speed(Motor_center_max(255-PS2_LY), Motor_center_max(255-PS2_LX),Motor_center_max(255-PS2_RY));
  }
	else if(name=='n')
	{
		change_speed(Motor_center_max(255-PS2_LY), Motor_center_max(255-PS2_LX),Motor_center_max(255-PS2_RY));
	}
}


int16_t UnsighTosigh (uint8_t law,uint8_t high)
{ 
	uint16_t y = high * 256 + law;
	printf("y:%d \r\n",y);
	int out=y;
	if(y >= 32768)
	{
		y--;
		y=~y;
		printf("~y:%u \r\n",y);
		out = -y;
		printf("out:%d \r\n",out);
			
	}
	return out ;
}

// urat ---------------------------------------------------------------------------


void HAL_UART_RxCpltCallback(UART_HandleTypeDef *UartHandle)
{
//	
// 		if(UartHandle->Instance == USART1)
// 		{

// 			if(Rxbuf[0]=='M')
// 			{
				
// 					printf("M is OK!");//���ڻش��ַ���M is ok
// 				//ͬʱ���ݴ��ڽ��յ��ĵڶ���������λ���ݿ������˳�������Ӧ���˶�
// 					change_speed((Rxbuf[1]-100)*9, (Rxbuf[2]-100)*9, (Rxbuf[3]-100)*9);
// 					printf("speed:%d,%d,%d",Rxbuf[1],Rxbuf[2],Rxbuf[3]);
// 			}
// 			else if(Rxbuf[0]=='S')//���յ������ݵĵ�һ���ַ�ΪSʱ
// 			{
// 					printf("S is OK!");
// 					change_speed(0,0,0);//���˳��ٶ�Ϊ0��ֹͣת��
// //					printf("speed:%d,%d,%d",Rxbuf[1],Rxbuf[2],Rxbuf[3]);
// 			}
// 		}

		if(UartHandle->Instance == USART2)//receive the motor speed
		{
			//rx_err++;
			if(rx_err)
			{
				if(rxmotor[0]==0x66)
				{
					rx_err=0;
				}
        //printf("err: %d",rxmotor[0]);
        		HAL_UART_Receive_DMA(&huart2,rxmotor,1);
				return;
			}
			if(rxmotor[0]==0x65&&rxmotor[5]==0x66)
			{
				for(int i=0;i<4;i++)
				{
					if(rxmotor[i+1]>128)
					{
						tar_speed[i]=-(rxmotor[i+1]-128);
					}
					else
					{
						tar_speed[i]=rxmotor[i+1];
					}
					
				}
				HAL_UART_Receive_DMA(&huart2,rxmotor,6);
				return;
			}
			if(rxmotor[0]==0x67&&rxmotor[5]==0x68)
			{
				for(int i=0;i<2;i++)
				{

					tar_list[i]=rxmotor[i+1];		
				}
				work=1;
				HAL_UART_Receive_DMA(&huart2,rxmotor,6);
			}
			else
			{
				rx_err=1;
				HAL_UART_Receive_DMA(&huart2,rxmotor,1);
			}
		}
  
}




/* USER CODE END 4 */

/* MPU Configuration */

void MPU_Config(void)
{
  MPU_Region_InitTypeDef MPU_InitStruct = {0};

  /* Disables the MPU */
  HAL_MPU_Disable();

  /** Initializes and configures the Region and the memory to be protected
  */
  MPU_InitStruct.Enable = MPU_REGION_ENABLE;
  MPU_InitStruct.Number = MPU_REGION_NUMBER0;
  MPU_InitStruct.BaseAddress = 0x0;
  MPU_InitStruct.Size = MPU_REGION_SIZE_4GB;
  MPU_InitStruct.SubRegionDisable = 0x87;
  MPU_InitStruct.TypeExtField = MPU_TEX_LEVEL0;
  MPU_InitStruct.AccessPermission = MPU_REGION_NO_ACCESS;
  MPU_InitStruct.DisableExec = MPU_INSTRUCTION_ACCESS_DISABLE;
  MPU_InitStruct.IsShareable = MPU_ACCESS_SHAREABLE;
  MPU_InitStruct.IsCacheable = MPU_ACCESS_NOT_CACHEABLE;
  MPU_InitStruct.IsBufferable = MPU_ACCESS_NOT_BUFFERABLE;

  HAL_MPU_ConfigRegion(&MPU_InitStruct);
  /* Enables the MPU */
  HAL_MPU_Enable(MPU_PRIVILEGED_DEFAULT);

}

/**
  * @brief  This function is executed in case of error occurrence.
  * @retval None
  */
void Error_Handler(void)
{
  /* USER CODE BEGIN Error_Handler_Debug */
  /* User can add his own implementation to report the HAL error return state */
  __disable_irq();
  while (1)
  {
  }
  /* USER CODE END Error_Handler_Debug */
}

#ifdef  USE_FULL_ASSERT
/**
  * @brief  Reports the name of the source file and the source line number
  *         where the assert_param error has occurred.
  * @param  file: pointer to the source file name
  * @param  line: assert_param error line source number
  * @retval None
  */
void assert_failed(uint8_t *file, uint32_t line)
{
  /* USER CODE BEGIN 6 */
  /* User can add his own implementation to report the file name and line number,
     ex: printf("Wrong parameters value: file %s on line %d\r\n", file, line) */
  /* USER CODE END 6 */
}
#endif /* USE_FULL_ASSERT */
